#include "mechanical_arm_task.h"
#include "mechanical_arm_behaviour.h"
#include "CAN_receive.h"
#include "fdcan.h"

// ???????????? 0??8191
#define ecd_format(ecd)         \
    {                           \
        if ((ecd) > ECD_RANGE)  \
            (ecd) -= ECD_RANGE; \
        else if ((ecd) < 0)     \
            (ecd) += ECD_RANGE; \
    }
		
#if INCLUDE_uxTaskGetStackHighWaterMark
uint32_t mechanical_arm_high_water;
#endif
		
mechanical_arm_control_t mechanical_arm_control;
		
static void mechanical_arm_init(mechanical_arm_control_t *init);
static void mechanical_arm_set_mode(mechanical_arm_control_t *set_mode);
static void mechanical_arm_feedback_update(mechanical_arm_control_t *feedback_update);
static void mechanical_arm_mode_change_control_transit(mechanical_arm_control_t *mechanical_arm_mode_change);
static void mechanical_arm_set_control(mechanical_arm_control_t *set_control);
static void mechanical_arm_control_loop(mechanical_arm_control_t *control_loop);

		
void mechanical_arm_task(void const *pvParameters)
{
    vTaskDelay(MACHANICAL_ARM_TASK_INIT_TIME);
		
    mechanical_arm_init(&mechanical_arm_control);

    while (1)
    {
        mechanical_arm_set_mode(&mechanical_arm_control);
        mechanical_arm_mode_change_control_transit(&mechanical_arm_control);
        mechanical_arm_feedback_update(&mechanical_arm_control);
        mechanical_arm_set_control(&mechanical_arm_control);
        mechanical_arm_control_loop(&mechanical_arm_control);

				mit_ctrl_joint(&hfdcan2, mechanical_arm_control.joint_motor[0].para.id, 0.0f, 3.14f, 0.0f, 0.5f, 0.0f);
				mit_ctrl_joint(&hfdcan2, mechanical_arm_control.joint_motor[1].para.id, 0.0f, 3.14f, 0.0f, 0.5f, 0.0f);
				mit_ctrl_joint(&hfdcan2, mechanical_arm_control.joint_motor[2].para.id, 0.0f, 3.14f, 0.0f, 0.5f, 0.0f);
				CAN_cmd_arm(0, 0, 0, 0);

        vTaskDelay(MACHANICAL_ARM_CONTROL_TIME);
			
#if INCLUDE_uxTaskGetStackHighWaterMark
        mechanical_arm_high_water = uxTaskGetStackHighWaterMark(NULL);
#endif
    }
}

const mechanical_arm_motor_t *get_j3_motor_point(void)
{
    return &mechanical_arm_control.mechanical_arm_j3_motor;
}

const mechanical_arm_motor_t *get_j4_motor_point(void)
{
    return &mechanical_arm_control.mechanical_arm_j4_motor;
}

const mechanical_arm_motor_t *get_j5_motor_point(void)
{
    return &mechanical_arm_control.mechanical_arm_j5_motor;
}

static void mechanical_arm_init(mechanical_arm_control_t *init)
{
		joint_motor_init(&init->joint_motor[0], 1, MIT_MODE);
		joint_motor_init(&init->joint_motor[1], 2, MIT_MODE);
		joint_motor_init(&init->joint_motor[2], 3, MIT_MODE);
		enable_motor_mode(&hfdcan2, init->joint_motor[0].para.id, init->joint_motor[0].mode);
		enable_motor_mode(&hfdcan2, init->joint_motor[1].para.id, init->joint_motor[1].mode);
		enable_motor_mode(&hfdcan2, init->joint_motor[2].para.id, init->joint_motor[2].mode);
		init->mechanical_arm_j3_motor.mechanical_arm_motor_measure = get_arm_j3_motor_measure_point();
    init->mechanical_arm_j4_motor.mechanical_arm_motor_measure = get_arm_j4_motor_measure_point();
		init->mechanical_arm_j5_motor.mechanical_arm_motor_measure = get_arm_j5_motor_measure_point();
}

static void mechanical_arm_set_mode(mechanical_arm_control_t *set_mode)
{
    if (set_mode == NULL)
    {
        return;
    }
    mechanical_arm_behaviour_mode_set(set_mode);
}

static void mechanical_arm_feedback_update(mechanical_arm_control_t *feedback_update)
{
    if (feedback_update == NULL)
    {
        return;
    }
}

static void mechanical_arm_mode_change_control_transit(mechanical_arm_control_t *mechanical_arm_mode_change)
{
    if (mechanical_arm_mode_change == NULL)
    {
        return;
    }
}

static void mechanical_arm_set_control(mechanical_arm_control_t *set_control)
{
    if (set_control == NULL)
    {
        return;
    }

}

static void mechanical_arm_control_loop(mechanical_arm_control_t *control_loop)
{
    if (control_loop == NULL)
    {
        return;
    }

}
